#!coding:utf-8
import numpy as np


def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config):
    """
    根据每只脚相对于腿身连接点的坐标计算出对应关节的角度
    Parameters
    ----------
    r_body_foot : [x,y,z]
        脚相对于腿身连接点的坐标
    leg_index : int
        第几条腿
    config : Configuration
        机器人配置参数

    Returns
    -------
    numpy array (3)
        对应腿上的三个关节角度
    """
    # 脚相对于身体连接点的坐标
    (x, y, z) = r_body_foot
    # OY=y, OB=z
    # R_body_foot_yz: 即BY长度
    R_body_foot_yz = (y ** 2 + z ** 2) ** 0.5

    # R_hip_foot_yz: 即HY, 在直角△BHY中计算
    R_hip_foot_yz = (R_body_foot_yz ** 2 - config.ABDUCTION_OFFSET ** 2) ** 0.5

    # phi: ∠HBY
    arccos_argument = config.ABDUCTION_OFFSETS[leg_index] / R_body_foot_yz
    # 防止溢出
    arccos_argument = np.clip(arccos_argument, -0.99, 0.99)
    phi = np.arccos(arccos_argument)

    # hip_foot_angle: ∠BYO
    hip_foot_angle = np.arctan2(z, y)

    # abduction_angle: 即α角 = ∠HBP = ∠HBY-∠OYB
    # 这里用加法是因为z始终是负数, 所以算出来的hip_foot_angle始终为负
    abduction_angle = phi + hip_foot_angle

    # theta: ∠YHF
    # x: YF, R_hip_foot_yz: YH
    # 正前方为x轴正向, 所以图中YF=-x
    theta = np.arctan2(-x, R_hip_foot_yz)
    # R_hip_foot: HF
    R_hip_foot = (R_hip_foot_yz ** 2 + x ** 2) ** 0.5

    # trident: ∠KHF, 在△KHF中由余弦定理计算
    # LEG_L1: HK, LEG_L2: KF, R_hip_foot: HF
    arccos_argument = (config.LEG_L1 ** 2 + R_hip_foot ** 2 - config.LEG_L2 ** 2) / (
        2 * config.LEG_L1 * R_hip_foot
    )
    # 防止溢出
    arccos_argument = np.clip(arccos_argument, -0.99, 0.99)
    trident = np.arccos(arccos_argument)

    # hip_angle: 即β角 = ∠YHF + ∠KHF
    hip_angle = theta + trident

    # beta: ∠HKF, △HKF中由余弦定理求得
    arccos_argument = (config.LEG_L1 ** 2 + config.LEG_L2 ** 2 - R_hip_foot ** 2) / (
        2 * config.LEG_L1 * config.LEG_L2
    )
    arccos_argument = np.clip(arccos_argument, -0.99, 0.99)
    beta = np.arccos(arccos_argument)

    # knee_angle: 即γ角 = ∠NKF = ∠QKN-∠QKF
    knee_angle = hip_angle - (np.pi - beta)

    return np.array([abduction_angle, hip_angle, knee_angle])


def four_legs_inverse_kinematics(r_body_foot, config):
    """
    根据给定四只脚的坐标计算每条腿对应关节的角度
    Parameters
    ----------
    r_body_foot : numpy array (3,4)
        每只脚相对于身体中心的坐标
    config : Config object
        机器人配置参数

    Returns
    -------
    numpy array (3,4)
        对应每个关节的角度
    """
    alpha = np.zeros((3, 4))
    for i in range(4):
        # 腿和身体连接点相对于身体中心的偏差
        body_offset = config.LEG_ORIGINS[:, i]
        # 计算每只脚相对于连接点的相对坐标
        alpha[:, i] = leg_explicit_inverse_kinematics(
            r_body_foot[:, i] - body_offset, i, config
        )
    return alpha
